#include <iostream>
#include <opencv2/opencv.hpp>
#include <queue>
#include <vector>
#include <stack>
#include <algorithm>
/****************************************
 **********移动机器人规划控制入门与实践*******
 **************DFS示例代码****************
 * 包含：
 * 图片转栅格地图；
 * DFS搜索算法；
 * 基于opencv的拓展过程显示。
 ****************************************
*/

//定义二维点
struct Point {
    int x;
    int y;
};

//辅助函数——标记障碍物
void markObstacles(cv::Mat &image, const std::vector<std::vector<int>> &grid) {
    for (int y = 0; y < grid.size(); y++) {
        for (int x = 0; x < grid[0].size(); x++) {
            if (grid[y][x] == 1) {
                // 使用黑色标记障碍物
                cv::Rect roi(x * 10, y * 10, 10, 10);
                image(roi) = cv::Vec3b(0, 0, 0); // Black color
            }
        }
    }
}
//辅助函数——标记起点
void markStart(cv::Mat &image, Point point) {
                // 使用蓝色标记起点
                cv::Rect roi(point.x * 10, point.y * 10, 10, 10);
                image(roi) = cv::Vec3b(255, 0, 0); // Blue color
}
//辅助函数——标记终点
void markGoal(cv::Mat &image, Point point) {
                // 使用深红标记终点
                cv::Rect roi(point.x * 10, point.y * 10, 10, 10);
                image(roi) = cv::Vec3b(0, 0, 125); // Red color       //Blue   Green   Red
}

//判断点是否在地图内
bool isValidPoint(const Point& p, int rows, int cols) {
    return (p.x >= 0 && p.x < cols && p.y >= 0 && p.y < rows);

}

std::vector<Point> dfsSearch(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image) {
    
    int rows = grid.size();
    int cols = grid[0].size();
    std::vector<std::vector<bool>> visited(rows, std::vector<bool>(cols, false));
    std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, { -1, -1 }));
    std::vector<Point> path;

    // int dx[] = {1, -1, 0, 0};   //右左下上
    // int dy[] = {0, 0, 1, -1};
    //  int dx[] = {1, -1, 0, 0};
    //  int dy[] = {0, 0, -1, 1};
    int dx[] = {1, 0, -1, 0};   //右上左下
    int dy[] = {0, -1, 0, 1};

    std::stack<Point> stack;
    stack.push(start);
    visited[start.y][start.x] = true;

    while (!stack.empty()) {
        Point current = stack.top();
        stack.pop();

        if (current.x == end.x && current.y == end.y) {
            // Found the end point, reconstruct the path
            Point temp = current;
            while (temp.x != start.x || temp.y != start.y) {
                path.push_back(temp);
                temp = parent[temp.y][temp.x];
            }
            path.push_back(start);
            std::reverse(path.begin(), path.end());
            return path;
        }

        for (int i = 0; i < 4; i++) {
            int nx = current.x + dx[i];
            int ny = current.y + dy[i];

            if (isValidPoint({nx, ny}, rows, cols) && !visited[ny][nx] && grid[ny][nx] == 0) {
                stack.push({nx, ny});
                visited[ny][nx] = true; // 标记已访问
                parent[ny][nx] = current; // 记录父节点

                if (nx == end.x && ny == end.y) {
                    // Found the end point, reconstruct the path
                    Point temp = {nx,ny};
                    while (temp.x != start.x || temp.y != start.y) {
                        path.push_back(temp);
                        temp = parent[temp.y][temp.x];
                    }
                    path.push_back(start);
                    std::reverse(path.begin(), path.end());
                    return path;
                }

                // 显示该节点
                if(!(nx==end.x && ny==end.y))
                {
                    cv::Rect roi(nx * 10, ny * 10, 10, 10);
                    image(roi) = cv::Vec3b(0, 255, 0); // Green color
                    cv::imshow("DFS Expansion", image);
                    cv::waitKey(1);
                }
            }
        }
    }

    // No path found
    return {};
}

/*无障碍物*/
std::string pic="0.png";   //设置地图
Point start = {40, 50};    // 设置起点
Point end = {70, 50};      //设置终点

// /*有障碍物*/
// std::string pic="1.png";   //设置地图
// Point start = {20, 50};    // 设置起点
// Point end = {88, 50};      //设置终点

int main() {
    // 读取图片
    cv::Mat image = cv::imread(pic);

    if (image.empty()) {
        std::cerr << "无法读取图像文件" << std::endl;
        return -1;
    }

    // 缩放图片到所需的尺寸（栅格地图的大小）
    cv::Size new_size(100, 100); // 根据需要调整大小
    cv::resize(image, image, new_size, 0, 0, cv::INTER_LINEAR);

    // 创建栅格地图，0表示可通行，1表示障碍
    std::vector<std::vector<int>> grid_map;

    for (int y = 0; y < new_size.height; y++) {
        std::vector<int> row;
        for (int x = 0; x < new_size.width; x++) {
            cv::Vec3b pixel = image.at<cv::Vec3b>(y, x);
            if (pixel == cv::Vec3b(0, 0, 0)) { // 假设黑色为障碍
                row.push_back(1);
            } else {
                row.push_back(0);
            }
        }
        grid_map.push_back(row);
    }

    // 打印栅格地图
    for (const auto& row : grid_map) {
        for (int value : row) {
            std::cout << value << " ";
        }
        std::cout << std::endl;
    }
    
    //坐标系定义：
    // Ori*---------->x
    //    |
    //    |
    //    |
    //    |
    //    v
    //    y

    // // 设置起、终点
    // Point start = {20, 50};
    // Point end = {88, 50};

    //显示地图
    cv::Mat output_image(grid_map.size() * 10, grid_map[0].size() * 10, CV_8UC3, cv::Scalar(255, 255, 255));
    markStart(output_image,start);
    markGoal(output_image,end);
    markObstacles(output_image, grid_map); // 标记障碍物

    //搜索路径
    std::vector<Point> path = dfsSearch(grid_map, start, end, output_image);

    cv::imshow("DFS Expansion", output_image);
    cv::waitKey(0); // Wait for user input

    //打印、显示路径
    if (path.empty()) {
        std::cout << "未找到路径" << std::endl;
    } else {
        std::cout << "找到路径，路径长度: " << path.size() << std::endl;
        //显示路径，但是不显示起点和终点
        for (unsigned int i=1;i<path.size()-1;i++) {
            std::cout << "(" << path[i].x << ", " << path[i].y << ") -> ";
            // Mark the path in red
            cv::Rect roi(path[i].x * 10, path[i].y * 10, 10, 10);
            output_image(roi) = cv::Vec3b(0, 0, 255); // Red color
        }
        std::cout << std::endl;
    }

    cv::imshow("DFS Expansion with Path", output_image);
    cv::waitKey(0); // Wait for user input

    return 0;
}

